/**
 * @file arm_robot.ino
 * @author yuhangsun
 * @brief 
 * @version 0.2
 * @date 2024-02-01、2024-04-07、2025-01-06、2025-01-22
 * 
 * @copyright Copyright (c) 2024
 * 
 */
#include <Wire.h>
#include <SoftwareSerial.h>
#include <Adafruit_NeoPixel.h>
#include "pin_definition.h"
#include <STM32FreeRTOS.h>

// 创建 TwoWire 类的实例
TwoWire WIRE(WIRE_SDA, WIRE_SCL);

//创建蓝牙控制对象
HardwareSerial BT_Serial(BLUETOOTH_UART);

// 定义结构体
typedef struct
{
  uint8_t servoID;
  uint8_t servoAngle;
} tServo_Components, *ptServo_Components;

// 建立数组
static tServo_Components Servo_Component[5] = {
  { .servoID = 9, .servoAngle = 30 },
  { .servoID = 10, .servoAngle = 95 },
  { .servoID = 11, .servoAngle = 10 },
  { .servoID = 12, .servoAngle = 50 },
  { .servoID = 13, .servoAngle = 95 }
};

/*start  add queues, ... */
/**队列模板，包括队列句柄定义；创建；发送；接收
  QueueHandle_t xxxx_QueueHandler = NULL;                       ///队列句柄定义
  xxxx_QueueHandler = xQueueCreate( 5, sizeof( long ) ); ///队列创建
  long lValueToSend;
  portBASE_TYPE xStatus;
  xStatus = xQueueSendToBack( xxxx_QueueHandler, &lValueToSend, 0 ); ///队列发送
  long lReceivedValue;
  xStatus = xQueueReceive( xxxx_QueueHandler, &lReceivedValue, xTicksToWait ); ///队列接收
*/
QueueHandle_t btCMD_QueueHandler = NULL;  ///队列句柄定义
/*end  add queues, ... */

//任务创建
void armBTCmd(void *pvParameters);
// void LED_Blink(void *pvParameters);

// 函数声明
bool I2CWrite(unsigned char reg_addr, unsigned char date);
void HardwareInit();
void resAngle();
void servoCmd();
void reportStatus();
void actionGrab();
void voiceBroadcast();
void sendData();
void RGB_Config();
void updateLEDColors();
void readColor();
void redAction();
void greenAction();
void blueAction();

//颜色识别相关参数定义
int val = 0;
boolean stat = I2C_COLOR_ADDR;
Adafruit_NeoPixel strip = Adafruit_NeoPixel(MAX_LED, LED_PIN, NEO_RGB + NEO_KHZ800);
unsigned int Red = 0;
unsigned int Green = 0;
unsigned int Blue = 0;
unsigned int val_red = 0;
unsigned int val_green = 0;
unsigned int val_blue = 0;


void setup() {
  HardwareInit();
  // 创建蓝牙控制任务
  xTaskCreate(
    armBTCmd,
    (const portCHAR *)"armBTCmd",
    1024,
    NULL,
    2,
    NULL);
  // xTaskCreate(
  //   LED_Blink,
  //   (const portCHAR *)"Blink",
  //   128,
  //   NULL,
  //   2,
  //   NULL);
  // btCMD_QueueHandler = xQueueCreate(16, sizeof(uint32_t));  ///队列创建
  // 启动调度器
  vTaskStartScheduler();
  Serial.println("Insufficient RAM");
  while (1);
}

void loop() {
}

// void LED_Blink(void *pvParameters) {
//   long xTicksToWait = portMAX_DELAY;
//   for (;;) {
//     uint32_t lReceivedValue;
//     portBASE_TYPE xStatus;
//     xStatus = xQueueReceive(btCMD_QueueHandler, &lReceivedValue, xTicksToWait);  ///队列接收
//     // Serial.println(lReceivedValue);
//     digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
//   }
// }
/****************************************************
函数名：armBTCmd
作用：通过蓝牙接收到的指令，选择不同的舵机并控制其角度。
参数：pvParameters 未使用，仅为满足 FreeRTOS 任务函数的要求
返回值：无
***********************************************/
void armBTCmd(void *pvParameters) {
  
  static long lValueToSend;
  static long lastTime = millis();
  for (;;) {
    if (BT_Serial.available() > 0) {
      char command = BT_Serial.read();

      switch (command) {
        case 'A':  // 增加角度，舵机1关爪子
          Servo_Component[0].servoAngle < 75 ? (Servo_Component[0].servoAngle += 1) : (Servo_Component[0].servoAngle = 75);
          I2CWrite(Servo_Component[0].servoID, Servo_Component[0].servoAngle);
          Serial.println("The Servo0 CLOSE");
          break;
        case 'D':  // 减小角度，舵机1开爪子
          Servo_Component[0].servoAngle > 50 ? (Servo_Component[0].servoAngle -= 1) : (Servo_Component[0].servoAngle = 50);
          I2CWrite(Servo_Component[0].servoID, Servo_Component[0].servoAngle);
          Serial.println("The Servo0 OPEN");
          break;
        case 'W':  // 减小角度，舵机2左转
          Servo_Component[1].servoAngle > 2 ? (Servo_Component[1].servoAngle -= 1) : (Servo_Component[1].servoAngle = 2);
          I2CWrite(Servo_Component[1].servoID, Servo_Component[1].servoAngle);
          Serial.println("The Servo1 UP");
          break;
        case 'S':  // 增加角度，舵机2右转
          Servo_Component[1].servoAngle < 178 ? (Servo_Component[1].servoAngle += 1) : (Servo_Component[1].servoAngle = 178);
          I2CWrite(Servo_Component[1].servoID, Servo_Component[1].servoAngle);
          Serial.println("The Servo1 DOWN");
          break;
        case 'w':  // 减小角度，舵机3前倾
          Servo_Component[2].servoAngle > 2 ? (Servo_Component[2].servoAngle -= 1) : (Servo_Component[2].servoAngle = 2);
          I2CWrite(Servo_Component[2].servoID, Servo_Component[2].servoAngle);
          Serial.println("The Servo2 UP");
          break;
        case 's':  // 增加角度，舵机3后仰
          Servo_Component[2].servoAngle < 178 ? (Servo_Component[2].servoAngle += 1) : (Servo_Component[2].servoAngle = 178);
          I2CWrite(Servo_Component[2].servoID, Servo_Component[2].servoAngle);
          Serial.println("The Servo2 DOWN");
          break;
        case 'a':  // 增加角度，舵机4前倾
          Servo_Component[3].servoAngle < 178 ? (Servo_Component[3].servoAngle += 1) : (Servo_Component[3].servoAngle = 178);
          I2CWrite(Servo_Component[3].servoID, Servo_Component[3].servoAngle);
          Serial.println("The Servo3 UP");
          break;
        case 'd':  // 减小角度，舵机4后仰
          Servo_Component[3].servoAngle > 2 ? (Servo_Component[3].servoAngle -= 1) : (Servo_Component[3].servoAngle = 2);
          I2CWrite(Servo_Component[3].servoID, Servo_Component[3].servoAngle);
          Serial.println("The Servo3 DOWN");
          break;
        case 'j':  // 增加角度，舵机5左转
          Servo_Component[4].servoAngle < 178 ? (Servo_Component[4].servoAngle += 1) : (Servo_Component[4].servoAngle = 178);
          I2CWrite(Servo_Component[4].servoID, Servo_Component[4].servoAngle);
          Serial.println("The Servo4 LEFT");
          break;
        case 'g':  // 减小角度，舵机5右转
          Servo_Component[4].servoAngle > 2 ? (Servo_Component[4].servoAngle -= 1) : (Servo_Component[4].servoAngle = 2);
          I2CWrite(Servo_Component[4].servoID, Servo_Component[4].servoAngle);
          Serial.println("The Servo4 RIGHT");
          break;
        case 'i':  // 复位
          resAngle();
          vTaskDelay(100 / portTICK_PERIOD_MS);
          voiceBroadcast(4);
          break;
        case 'o':  // 打印
          reportStatus();
          vTaskDelay(100 / portTICK_PERIOD_MS);
          voiceBroadcast(5);
          break;
        case 'p':  // 一键抓取
          actionGrab();
          vTaskDelay(100 / portTICK_PERIOD_MS);
          voiceBroadcast(9);
          break;
        case '1':  // 颜色识别
          updateLEDColors();
          vTaskDelay(100 / portTICK_PERIOD_MS);
          break;
        case '2':
          voiceBroadcast(3);
          break;
        case '3':
          voiceBroadcast(4);
          break;
        default:
          break;
      }
    }
    vTaskDelay(10 / portTICK_PERIOD_MS);  // 适当延迟，避免 CPU 占用过高
    /**
    这个示例，每隔200ms通过队列btCMD_QueueHandler向任务LED Blink发送一个lValueToSend数值
    lValueToSend可以改成自己的数据(例如结构体或者指针)，但是要在初始化里也修改传输数据的大小
    接收也要同时修改
    通过这个例子，你可以自己建立两个任务的通信传输。我在前面放了一个队列的初始化模板，你可以复制修改创建自己队列
    */
    // portBASE_TYPE xStatus;
    // if ((millis() - lastTime) > 200) {
    //   lValueToSend++;
    //   lastTime= millis();
    //   xStatus = xQueueSendToBack(btCMD_QueueHandler, &lValueToSend, 0);  ///队列发送
    // }
  }
}


/****************************
      寄存器设置函数  
*****************************/
bool I2CWrite(unsigned char reg_addr, unsigned char date) {
  DEV_I2C.beginTransmission(I2C_ROBOT_ADDR);  //发送Device地址
  DEV_I2C.write(reg_addr);                    //发送要操作的舵机
  DEV_I2C.write(date);                        //发送要设置的角度
  if (DEV_I2C.endTransmission() != 0)         //发送结束信号
  {
    vTaskDelay(10 / portTICK_PERIOD_MS);
    return false;
  }
  vTaskDelay(10 / portTICK_PERIOD_MS);
  return true;
}

/******************************************************
函数名：HardwareInit
作用：初始化函数
参数：无
返回值：无
********************************************************/
void HardwareInit() {
  Serial.begin(115200);
  Serial.println("+ Welcome to YeMing Robot +");
  BT_Serial.begin(115200);
  BT_Serial.println("+ Welcome to YeMing Robot +");
  DEV_I2C.begin();
  RGB_Config();
  strip.begin();  // 初始化 Adafruit_NeoPixel；
  strip.show();   // 显示所有 LED 为关状态;
  // pinMode(LED_BUILTIN, OUTPUT);
  pinMode(JQ8900, OUTPUT);
  // 播报第 3 条语音
  // voiceBroadcast(3);
}

/******************************************************
函数名：resAngle
作用：复位舵机角度
参数：无
返回值：无
********************************************************/
void resAngle() {
  Serial.println("Restore Initial Position.");
  BT_Serial.println("Restore Initial Position.");
  I2CWrite(9, 50);  // 设置舵机 1 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(10, 95);  // 设置舵机 2 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(11, 10);  // 设置舵机 3 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(12, 50);  // 设置舵机 4 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
}

/****************************************************
函数名：reportStatus
作用：舵机信息打印
参数：pvParameters 未使用，仅为满足 FreeRTOS 任务函数的要求
返回值：无
***********************************************/
void reportStatus() {
  Serial.println("");
  Serial.println("");
  Serial.println("+ Robot-Arm Status Report +");
  Serial.print("Servo1 Position: ");
  Serial.println(Servo_Component[0].servoAngle);
  Serial.print("Servo2 Position: ");
  Serial.println(Servo_Component[1].servoAngle);
  Serial.print("Servo3 Position:");
  Serial.println(Servo_Component[2].servoAngle);
  Serial.print("Servo4 Position:");
  Serial.println(Servo_Component[3].servoAngle);
  Serial.print("Servo5 Position:");
  Serial.println(Servo_Component[4].servoAngle);
  Serial.println("++++++++++++++++++++++++++");
  Serial.println("");

  BT_Serial.println("");
  BT_Serial.println("");
  BT_Serial.println("+ Robot-Arm Status Report +");
  BT_Serial.print("Servo1 Position: ");
  BT_Serial.println(Servo_Component[0].servoAngle);
  BT_Serial.print("Servo2 Position: ");
  BT_Serial.println(Servo_Component[1].servoAngle);
  BT_Serial.print("Servo3 Position:");
  BT_Serial.println(Servo_Component[2].servoAngle);
  BT_Serial.print("Servo4 Position:");
  BT_Serial.println(Servo_Component[3].servoAngle);
  BT_Serial.print("Servo5 Position:");
  BT_Serial.println(Servo_Component[4].servoAngle);
  BT_Serial.println("++++++++++++++++++++++++++");
  BT_Serial.println("");
  vTaskDelay(1000 / portTICK_PERIOD_MS);  // 适当延迟，避免频繁打印
}

/******************************************************
函数名：actionGrab
作用：机械臂实现一键抓取功能
参数：pvParameters 未使用，仅为满足 FreeRTOS 任务函数的要求
返回值：无
********************************************************/
void actionGrab() {
  Serial.println("ActionGrab is sufficient");
  BT_Serial.println("ActionGrab is sufficient");
  
  // 回
  I2CWrite(9, 50);  // 设置舵机 1 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(10, 95);  // 设置舵机 2 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(11, 10);  // 设置舵机 3 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(12, 50);  // 设置舵机 4 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(1000 / portTICK_PERIOD_MS);
  // 抓
  I2CWrite(13, 98);  // 设置舵机 5 角度
  vTaskDelay(1000 / portTICK_PERIOD_MS);
  I2CWrite(11, 11);  // 设置舵机 3 角度
  vTaskDelay(1000 / portTICK_PERIOD_MS);
  I2CWrite(10, 97);  // 设置舵机 2 角度
  vTaskDelay(1000 / portTICK_PERIOD_MS);
  I2CWrite(12, 146);  // 设置舵机 4 角度
  vTaskDelay(1000 / portTICK_PERIOD_MS);
  I2CWrite(9, 70);  // 设置舵机 1 角度
  vTaskDelay(1500 / portTICK_PERIOD_MS);
  // 回
  I2CWrite(9, 70);  // 设置舵机 1 角度
  vTaskDelay(1000 / portTICK_PERIOD_MS);
  I2CWrite(10, 100);  // 设置舵机 2 角度
  vTaskDelay(1000 / portTICK_PERIOD_MS);
  I2CWrite(11, 10);  // 设置舵机 3 角度
  vTaskDelay(1000 / portTICK_PERIOD_MS);
  I2CWrite(12, 50);  // 设置舵机 4 角度
  vTaskDelay(1000 / portTICK_PERIOD_MS);
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(1000 / portTICK_PERIOD_MS);
  // 放
  I2CWrite(13, 148);  // 设置舵机 5 角度
  vTaskDelay(1500 / portTICK_PERIOD_MS);
  I2CWrite(12, 140);  // 设置舵机 4 角度
  vTaskDelay(1500 / portTICK_PERIOD_MS);
  I2CWrite(11, 22);  // 设置舵机 3 角度
  vTaskDelay(1500 / portTICK_PERIOD_MS);
  I2CWrite(10, 97);  // 设置舵机 2 角度
  vTaskDelay(1500 / portTICK_PERIOD_MS);
  I2CWrite(9, 50);  // 设置舵机 1 角度
  vTaskDelay(1500 / portTICK_PERIOD_MS);
  // 回
  I2CWrite(9, 50);  // 设置舵机 1 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(10, 95);  // 设置舵机 2 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(11, 10);  // 设置舵机 3 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(12, 50);  // 设置舵机 4 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
}


/******************************************************
函数名：sendData
作用：用于向 JQ8900 音频模块发送数据
返回值：addr
********************************************************/
void sendData(char addr)  // 发送函数。
{
  digitalWrite(JQ8900, HIGH); /*开始拉高*/
  vTaskDelay(pdMS_TO_TICKS(1));
  digitalWrite(JQ8900, LOW); /*开始引导码*/
  vTaskDelay(pdMS_TO_TICKS(3.2));
  for (int i = 0; i < 8; i++) /*总共 8 位数据  */
  {
    digitalWrite(JQ8900, HIGH);
    if (addr & 0x01) /*3:1 表示数据位 1,每个位用两个脉冲表示  */
    {
      vTaskDelay(pdMS_TO_TICKS(0.6));
      digitalWrite(JQ8900, LOW);
      vTaskDelay(pdMS_TO_TICKS(0.2));
    } else /*1：3 表示数据位 0,每个位用两个脉冲表示  */
    {
      vTaskDelay(pdMS_TO_TICKS(0.2));
      digitalWrite(JQ8900, LOW);
      vTaskDelay(pdMS_TO_TICKS(0.6));
    }
    addr >>= 1;
  }
  digitalWrite(JQ8900, HIGH);
  vTaskDelay(pdMS_TO_TICKS(1));
}

/******************************************************
函数名：voiceBroadcast
作用：语音播报
参数：曲目号
返回值：无
内容说明：
语音 1 内容：开始抓取
语音 2 内容：蓝牙已连接，蓝牙遥控模式启动
语音 3 内容：机械臂启动，请连接蓝牙
语音 4 内容：机械臂已复位
语音 5 内容：舵机角度打印成功，请在app对话框中查看
语音 6 内容：物体颜色为红色，开始抓取
语音 7 内容：物体颜色为绿色，开始抓取
语音 8 内容：物体颜色为蓝色，开始抓取
语音 9 内容：抓取成功
********************************************************/
void voiceBroadcast(int trackNumber) {
  // 设置音量为 30
  sendData(0x0a);  // 清空数字
  sendData(0x03);
  sendData(0x00);
  sendData(0x0c);
  vTaskDelay(pdMS_TO_TICKS(100));  // 延时
  // 选曲播放
  sendData(0x0a);         // 清空数字
  sendData(trackNumber);  // 曲目号
  sendData(0x0b);         // 选曲播放
  vTaskDelay(pdMS_TO_TICKS(100));
  // 开始播放
  sendData(0x11);
  vTaskDelay(pdMS_TO_TICKS(100));
}

/*****************************
函数名：RGB_Config
作用：配置与 RGB 相关的设备寄存器，设置复位、积分时间以及开启 CRGB 通道等参数
参数：无
返回值：无
******************************/
void RGB_Config(void) {
  uint8_t W_ADDR;

  // 复位寄存器，200ms 积分时间，CRGB 通道全部开启
  DEV_I2C.beginTransmission(I2C_COLOR_ADDR);  // 发送 Device 地址
  DEV_I2C.write(ControlReg_Addr);
  DEV_I2C.write(0x06);
  DEV_I2C.endTransmission();  // 发送结束信号

  DEV_I2C.beginTransmission(I2C_COLOR_ADDR);  // 发送 Device 地址
  DEV_I2C.write(0x04);
  DEV_I2C.write(0x41);        // 16bit 50ms
  DEV_I2C.endTransmission();  // 发送结束信号

  DEV_I2C.beginTransmission(I2C_COLOR_ADDR);  // 发送 Device 地址
  DEV_I2C.write(0x05);
  DEV_I2C.write(0x01);
  DEV_I2C.endTransmission();  // 发送结束信号
}

/*****************************
函数名：readColor
作用：通过 I2C 通信从传感器获取颜色数据，并对数据进行处理、校准、映射，最终判断并输出主要颜色。
参数：无
返回值：无
******************************/
void readColor() {
  int index = 0;
  char ColorData[6] = { 0 };
  // 红色
  DEV_I2C.beginTransmission(I2C_COLOR_ADDR);  // 发送 Device 地址
  DEV_I2C.write(0x10);
  DEV_I2C.endTransmission();                  // 发送结束信号
  DEV_I2C.beginTransmission(I2C_COLOR_ADDR);  // 发送 Device 地址
  DEV_I2C.requestFrom(I2C_COLOR_ADDR, 2);     // request 1 bytes from slave device #2
  while (DEV_I2C.available())                 // slave may send less than requested
  {
    char ff = DEV_I2C.read();  // receive a byte as character
    ColorData[index] = ff;
    index++;
  }
  DEV_I2C.endTransmission();  // 发送结束信号

  // 绿色
  DEV_I2C.beginTransmission(I2C_COLOR_ADDR);  // 发送 Device 地址
  DEV_I2C.write(0x0D);
  DEV_I2C.endTransmission();                  // 发送结束信号
  DEV_I2C.beginTransmission(I2C_COLOR_ADDR);  // 发送 Device 地址
  DEV_I2C.requestFrom(I2C_COLOR_ADDR, 2);     // request 1 bytes from slave device #2
  while (DEV_I2C.available())                 // slave may send less than requested
  {
    char ff = DEV_I2C.read();  // receive a byte as character
    ColorData[index] = ff;
    index++;
  }
  DEV_I2C.endTransmission();  // 发送结束信号

  // 蓝色
  DEV_I2C.beginTransmission(I2C_COLOR_ADDR);  // 发送 Device 地址
  DEV_I2C.write(0x13);
  DEV_I2C.endTransmission();                  // 发送结束信号
  DEV_I2C.beginTransmission(I2C_COLOR_ADDR);  // 发送 Device 地址
  DEV_I2C.requestFrom(I2C_COLOR_ADDR, 2);     // request 1 bytes from slave device #2
  while (DEV_I2C.available())                 // slave may send less than requested
  {
    char ff = DEV_I2C.read();  // receive a byte as character
    ColorData[index] = ff;
    index++;
  }
  DEV_I2C.endTransmission();                                                    // 发送结束信号
  Red = ((unsigned int)(ColorData[1] & 0xff) << 8 | (ColorData[0] & 0xff));  // 2.06
  Green = ((unsigned int)(ColorData[3] & 0xff) << 8 | (ColorData[2] & 0xff));
  Blue = ((unsigned int)(ColorData[5] & 0xff) << 8 | (ColorData[4] & 0xff));


  if (Red > 4500) Red = 4500;  // 4500.7600.4600 分别是检测到纯白色纸张时，所检测到的校准值，如果颜色识别不准确可以修改这里的校准值。
  if (Green > 7600) Green = 7600;
  if (Blue > 4600) Blue = 4600;
  val_red = map(Red, 0, 4500, 0, 255);
  val_blue = map(Blue, 0, 4600, 0, 255);
  val_green = map(Green, 0, 7600, 0, 255);


  if (val_red > val_green && val_red > val_blue) {
    val_red = 255;
    val_green /= 2;
    val_blue /= 2;
    Serial.println("R");
    vTaskDelay(pdMS_TO_TICKS(100));
    voiceBroadcast(6);
    vTaskDelay(pdMS_TO_TICKS(1000));
    redAction();
    vTaskDelay(pdMS_TO_TICKS(1000));
    voiceBroadcast(9);
    vTaskDelay(pdMS_TO_TICKS(100));

  } else if (val_green > val_red && val_green > val_blue) {
    val_green = 255;
    val_red /= 2;
    val_blue /= 2;
    Serial.println("G");
    vTaskDelay(pdMS_TO_TICKS(100));
    voiceBroadcast(7);
    vTaskDelay(pdMS_TO_TICKS(1000));
    greenAction();
    vTaskDelay(pdMS_TO_TICKS(1000));
    voiceBroadcast(9);
    vTaskDelay(pdMS_TO_TICKS(100));
  } else {
    val_blue = 255;
    val_red /= 2;
    val_green /= 2;
    Serial.println("B");
    vTaskDelay(pdMS_TO_TICKS(100));
    voiceBroadcast(8);
    vTaskDelay(pdMS_TO_TICKS(1000));
    blueAction();
    vTaskDelay(pdMS_TO_TICKS(1000));
    voiceBroadcast(9);
    vTaskDelay(pdMS_TO_TICKS(100));
  }

  vTaskDelay(pdMS_TO_TICKS(1000));
}

/********************************
函数名：updateLEDColors
作用：根据颜色数据更新 NeoPixel 灯带显示
参数：无
返回值：无
***********************************/
void updateLEDColors() {
  readColor();
  uint8_t i = 0, a = 0;
  uint32_t color = strip.Color(val_green, val_red, val_blue);  // 绿红蓝
  while (a < MAX_LED) {
    for (i = 0; i < MAX_LED; i++) {
      strip.setPixelColor(i, color);
    }
    strip.show();
    a++;
  }
  vTaskDelay(pdMS_TO_TICKS(100));
}


/*******************
函数名：serialEvent
作用：处理串口事件，读取串口数据并打印
参数：无
返回值：无
***********************/
void serialEvent() {
  while (Serial.available()) {
    // 一个字节一个字节地读,下一句是读到的放入字符串数组中组成一个完成的数据包
    char IncomingByte = Serial.read();
    Serial.print(IncomingByte);
  }
  vTaskDelay(pdMS_TO_TICKS(10));
}

/********************
函数名：redAction
作用：识别红色以后的舵机运动
参数：无
返回值：无
*****************************/
void redAction() {
  I2CWrite(9, 50);  // 设置舵机 1 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(10, 95);  // 设置舵机 2 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(11, 10);  // 设置舵机 3 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(12, 50);  // 设置舵机 4 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  // 抓
  I2CWrite(13, 25);  // 设置舵机 5 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(11, 5);  // 设置舵机 3 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(10, 100);  // 设置舵机 2 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(12, 145);  // 设置舵机 4 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(9, 70);  // 设置舵机 1 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  // 回
  I2CWrite(9, 70);  // 设置舵机 1 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(10, 100);  // 设置舵机 2 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(11, 10);  // 设置舵机 3 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(12, 50);  // 设置舵机 4 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  // 放
  I2CWrite(13, 50);  // 设置舵机 5 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(12, 143);  // 设置舵机 4 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(11, 5);  // 设置舵机 3 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(10, 100);  // 设置舵机 2 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(9, 30);  // 设置舵机 1 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  // 回
  I2CWrite(9, 50);  // 设置舵机 1 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(10, 95);  // 设置舵机 2 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(11, 10);  // 设置舵机 3 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(12, 50);  // 设置舵机 4 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
}

/********************
函数名：blueAction
作用：识别蓝色以后的舵机运动
参数：无
返回值：无
*****************************/
void blueAction() {
  I2CWrite(9, 50);  // 设置舵机 1 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(10, 95);  // 设置舵机 2 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(11, 10);  // 设置舵机 3 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(12, 50);  // 设置舵机 4 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  // 抓
  I2CWrite(13, 25);  // 设置舵机 5 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(11, 5);  // 设置舵机 3 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(10, 100);  // 设置舵机 2 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(12, 145);  // 设置舵机 4 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(9, 70);  // 设置舵机 1 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  // 回
  I2CWrite(9, 70);  // 设置舵机 1 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(10, 100);  // 设置舵机 2 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(11, 10);  // 设置舵机 3 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(12, 50);  // 设置舵机 4 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  // 放
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(12, 143);  // 设置舵机 4 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(11, 5);  // 设置舵机 3 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(10, 100);  // 设置舵机 2 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(9, 30);  // 设置舵机 1 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  // 回
  I2CWrite(9, 50);  // 设置舵机 1 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(10, 95);  // 设置舵机 2 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(11, 10);  // 设置舵机 3 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(12, 50);  // 设置舵机 4 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
}

/********************
函数名：greenAction
作用：识别绿色以后的舵机运动
参数：无
返回值：无
*****************************/
void greenAction() {
  I2CWrite(9, 50);  // 设置舵机 1 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(10, 95);  // 设置舵机 2 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(11, 10);  // 设置舵机 3 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(12, 50);  // 设置舵机 4 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  // 抓
  I2CWrite(13, 25);  // 设置舵机 5 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(11, 5);  // 设置舵机 3 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(10, 100);  // 设置舵机 2 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(12, 145);  // 设置舵机 4 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(9, 70);  // 设置舵机 1 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  // 回
  I2CWrite(9, 70);  // 设置舵机 1 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(10, 100);  // 设置舵机 2 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(11, 10);  // 设置舵机 3 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(12, 50);  // 设置舵机 4 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  // 放
  I2CWrite(13, 120);  // 设置舵机 5 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(12, 143);  // 设置舵机 4 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(11, 5);  // 设置舵机 3 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(10, 100);  // 设置舵机 2 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  I2CWrite(9, 50);  // 设置舵机 1 角度
  vTaskDelay(pdMS_TO_TICKS(1000));
  // 回
  I2CWrite(9, 50);  // 设置舵机 1 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(10, 95);  // 设置舵机 2 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(11, 10);  // 设置舵机 3 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(12, 50);  // 设置舵机 4 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
  I2CWrite(13, 95);  // 设置舵机 5 角度
  vTaskDelay(500 / portTICK_PERIOD_MS);
}
